perm filename CAMERA.OLD[SYS,HE] blob
sn#052036 filedate 1973-07-03 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00022 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 BEGIN "GILCAM"
C00006 00003 ⊃ GLOBAL STORAGE
C00008 00004 ⊃ MISC. PROCEDURES
C00010 00005 ⊃ READS DATA IN GILL'S FORMAT
C00012 00006 ⊃ SERVO THE SELECTED CAMERA
C00015 00007 ⊃ Compute the transformation matrices for the camera model
C00018 00008 ⊃ CONTINUE COMPUTING TRANSFORM
C00020 00009 ⊃ UPDATE THE TRANSFORM FOR SELECTED CAMERA
C00022 00010 ⊃ UPDATE CONTINUES
C00024 00011 ⊃ UPDATE CONTINUES
C00027 00012 ⊃ UPDATE CONTINUES
C00029 00013 ⊃ LENS CHANGES
C00030 00014 ⊃ MOVE PAN/TILT HEAD
C00032 00015 ⊃ CHANGE LENS FOCUS AND ZOOM
C00035 00016 ⊃ CENTER CAMERA
C00038 00017 ⊃ CAMERA CENTERING CONTINUES
C00040 00018 ⊃ TRANSFORM PREDICTION
C00043 00019 ⊃ CONTINUE PREDICTING, AND INITIALIZATION ROUTINE
C00045 00020 ⊃ PROGRAM TEST ROUTINE
C00047 00021 ⊃ CONTINUE TESTING
C00049 00022 ⊃ THIS IS THE MAIN PROGRAM
C00050 ENDMK
C⊗;
BEGIN "GILCAM"
REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
REQUIRE "HELIB[1,3]" LIBRARY;
require "drvpgm[sys,he]" load_module;
REQUIRE -1 NEW_ITEMS;
DEFINE ⊃="COMMENT";
⊃ THIS IS THE CAMERA MINDER . IT HOUSES THE FOLLOWING MESSAGE PROCEDURES:
CAM_INIT-INITIALIZES THE CAMERA MODEL FROM ONE OF IRWIN'S RELAXED MODELS.
CAM_UPDATEATE-READS THE POTS AND CREATES A NEW CAMERA TRANSFORM MATRIX.
CHNG_LENS(LENS), MOVE_CAM(PAN,TILT), CHNG_FOCUS(RANGE)-DO THE OBVIOUS.
CAM_CENTER(LENS,X,Y,Z)-CENTER THE CAMERA ON TABLE COORDS X,Y,Z.
CAM_PRED(LENS,X,Y,Z)-COMPUTE THE MODEL FOR THE CAMERA ON TABLE COORDS
X,Y,Z, AND STORE IT INTO CAMERA_PREDI.
THE CAMERA TRANSFROM MATRIX IS A 10X3 MATRIX:
1:3 X 1:3 THE COLINEATION MATRIX.
4 X 1:3 THE LENS CENTER.
5 X 1:3 THE CENTER OF THE FIELD OF VIEW
6:8 X 1:3 THE INVERSE COLINEATION MATRIX.
9 X 1:3 CAMPAN, CAMTIL, CAMRANG
10 X 1:2 CAMERA NO., CAMLENS
A NEW ITEM IS GENERATED, PUT IN CURCAM, AND THIS MATRIX BECOMES
ITS DATUM.
ERRORS: 1-SERVO PROBLEMS(SERVO).
2-LENS OUT OF BOUNDS(CHNG_LENS,CAM_CENTER).
3-PAN OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
4-TILT OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
5-FOCUS OUT OF BOUNDS(CHNG_FOCUS,CAM_CENTER).
6-LENS CHANGER GOOFED(CHNG_LENS).
7-LOOKUP FAILED(DATXFR).
8-NOT ENOUGH READINGS(CAM_UPDATE).
9-POTS TOO NOISY(CAM_UPDATE).
10-AD NOT AVAILABLE(CAM_UPDATE).
;
⊃ GLOBAL STORAGE;
REAL ARRAY MCOL,MICOL [1:3,1:3]; ⊃ WHERE I KEEP CURRENT CAMERA XFORM;
REAL ARRAY LCEN[1:3]; ⊃ AND LENS CENTER ;
REAL ZPOT0,ZPOTD,REF,FOC_LGTH;
EXTERNAL REAL SREF,CREF;
REAL ARRAY PPOT0,PPOTD,TPOT0,TPOTD,FPOT0,FPOTD,MART,SWING,GROREF,
FOC,FOCLEN0,FOCLENG[1:5],DP,P0[1:5,1:3],PP[1:5,1:2];
INTEGER MESS,BLOCK,DUMMY,EOF;
DEFINE XDATA="3",STX="1 STEP 1 UNTIL",CRLF="&'15&'12",
YES="INCHWL=""Y""",KY="684.5",C1="2.1249",C2="-.0006";
preload_with 2000.,4096.;
real array plimit1[1:2];
preload_with -2000.,-4096.;
real array plimit2[1:2];
preload_with -340.,4096.;
real array tlimit1[1:2];
preload_with -1870.,-4096.;
real array tlimit2[1:2];
preload_with 1900.,4096.;
real array flimit1[1:2];
preload_with 0.0,-4096.;
real array flimit2[1:2];
⊃ MISC. PROCEDURES;
SIMPLE PROCEDURE HAT(INTEGER CAMNO);
BEGIN
EXTERNAL PROCEDURE CALLEN;
EXTERNAL INTEGER LENS;
IF CAMNO=1 THEN BEGIN
CALLEN;
CAMLENS←LENS+1;
END;
CAM_UPDATE(CAMNO);
END "HAT";
⊃ Solves the eq. Acos(x)+Bsin(x)+C=0 for x;
SIMPLE REAL PROCEDURE COSQR(REAL A,B,C);
BEGIN
FORTRAN REAL PROCEDURE ACOS(REAL X);
FORTRAN REAL PROCEDURE SQRT(REAL X);
REAL K,M,N;
K←A*C;
M←A↑2+B↑2;
N←B↑2-C↑2;
IF B<0 THEN RETURN(ACOS(-(K/M)+SQRT((K/M)↑2+(N/M))))
ELSE RETURN(ACOS(-(K/M)-SQRT((K/M)↑2+(N/M))));
END "COSQR";
SIMPLE REAL PROCEDURE LNS(REAL X; INTEGER RLENS);
RETURN (X*FOC[RLENS]/(X-FOC[RLENS]));
⊃ READS DATA IN GILL'S FORMAT;
SIMPLE PROCEDURE DATXFR;
BEGIN
BOOLEAN FLAG;
INTEGER DUMMY;
REQUIRE "⊂⊃⊂⊃" DELIMITERS;
DEFINE DATASET= ⊂(IF CAMNUM=1 THEN "DATA[SYS,HE]"
ELSE "DATA2[SYS,HE]")⊃;
REQUIRE UNSTACK_DELIMITERS;
DEFINE XFR(X)="ARRYIN(XDATA,X,1)";
OPEN(XDATA,"DSK",12,3,0,0,0,0);
LOOKUP(XDATA,DATASET,FLAG);
IF FLAG THEN
BEGIN
OUTSTR("DATXFR-FAILED: LOOKUP FAILED FOR "&DATASET CRLF);
CAMFLG←7;
RETURN;
END;
IF TYP_CAM THEN OUTSTR("DATXFR: RETRIEVING "&DATASET&CVS(BLOCK)CRLF);
USETI(XDATA,1);
DUMMY←WORDIN(XDATA);
USETI(XDATA,BLOCK);
IF CAMNUM=2 THEN
BEGIN BLOCK←5; XFR(ZPOT0); XFR(ZPOTD); END;
XFR(PPOT0[BLOCK]);
XFR(PPOTD[BLOCK]);
XFR(TPOT0[BLOCK]);
XFR(TPOTD[BLOCK]);
XFR(FPOT0[BLOCK]);
XFR(FPOTD[BLOCK]);
XFR(MART[BLOCK]);
XFR(SWING[BLOCK]);
XFR("PP[BLOCK,1]");
XFR("PP[BLOCK,2]");
XFR("P0[BLOCK,1]");
XFR("P0[BLOCK,2]");
XFR("P0[BLOCK,3]");
XFR("DP[BLOCK,1]");
XFR("DP[BLOCK,2]");
XFR("DP[BLOCK,3]");
XFR(FOC[BLOCK]);
XFR(FOCLEN0[BLOCK]);
XFR(FOCLENG[BLOCK]);
XFR(GROREF[BLOCK]);
RELEASE(XDATA);
END "DATXFR";
⊃ SERVO THE SELECTED CAMERA;
SIMPLE PROCEDURE SERVO;
BEGIN
EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
EXTERNAL PROCEDURE SSERVO;
EXTERNAL REAL L1,L2,L3,L4,L5,L6,L7,L8,e1,e2,e3;
EXTERNAL REAL P1,P2,P3,P4,P5,P6,P7,P8;
EXTERNAL INTEGER STATUS,TSERVO;
EXTERNAL INTEGER STATE;
EXTERNAL PROCEDURE SPWOFF;
IF CAMNUM=1 THEN
BEGIN
SPWON(1,TSERVO);
WHILE ¬(STATUS LAND 1) DO;
IF (STATUS≥'100)∧(STATUS<'100000) THEN
BEGIN
OUTSTR("SERVO-FAILED: STATUS="&CVOS(STATUS)CRLF);
CAMFLG←1;
END;
SPWOFF;
CALL(1,"SLEEP");
IF DEB_CAM THEN
BEGIN
OUTSTR("SERVO: E1="&CVF(E1)&" E2="&CVF(E2)&
" E3="&CVF(E3)CRLF);
OUTSTR(" L1="&CVF(L1)&" L2="&CVF(L2)&
" L3="&CVF(L3)CRLF);
OUTSTR(" P1="&CVF(P1)&" P2="&CVF(P2)&
" P3="&CVF(P3)CRLF);
END;
END ELSE IF CAMNUM=2 THEN BEGIN
SSERVO;
WHILE STATE=0 DO;
IF (STATE≥'100)∧(STATE<'100000) THEN
BEGIN
OUTSTR("SIERRA SERVO FAILED: STATUS="&
CVOS(STATE)CRLF);
CAMFLG←1;
END;
IF DEB_CAM THEN
BEGIN
OUTSTR("SERVO: L4="&CVF(L4)&" L5="&CVF(L5)&
" L6="&CVF(L6)&" L7="&CVF(L7)&
" L8="&CVF(L8)CRLF);
OUTSTR(" P4="&CVF(P4)&" P5="&CVF(P5)&
" P6="&CVF(P6)&" P7="&CVF(P7)&
" P8="&CVF(P8)CRLF);
END;
END;
END "SERVO";
⊃ Compute the transformation matrices for the camera model
given by LENS and the last pots readings;
PROCEDURE PANTIL_CAM(INTEGER CLENS;REAL PPOT,TPOT,FPOT,ZPOT;
REAL ARRAY COL,ICOL,CENTER);
BEGIN
INTEGER I,J;
REAL ACC,FMX,FMY,PAN,TILT;
REAL ARRAY RP,RT,RPT,RS,R[1:3,1:3],C[1:3];
FORTRAN REAL PROCEDURE SIN;
FORTRAN REAL PROCEDURE COS;
REQUIRE "SOBMAT[SYS,HE]" LOAD_MODULE;
EXTERNAL PROCEDURE INVRT(REAL ARRAY A,AI);
PAN←PPOTD[CLENS]*PPOT+PPOT0[CLENS];
TILT←TPOTD[CLENS]*TPOT+TPOT0[CLENS];
FMY←FPOTD[CLENS]*FPOT+FPOT0[CLENS];
IF CAMNUM=2 THEN FMY←FMY+ZPOTD/(ZPOT-ZPOT0);
FMX←FMY*MART[CLENS];
RP[2,3]←-1;
RPT[1,1]←RP[1,1]←RP[3,2]←-SIN(PAN);
RP[3,1]←-(RPT[1,2]←RP[1,2]←COS(PAN));
RT[1,1]←1;
RPT[2,3]←-(RT[2,2]←RT[3,3]←COS(TILT));
R[3,3]←RPT[3,3]←RT[2,3]←-(RT[3,2]←SIN(TILT));
RPT[2,1]←RT[2,3]*RP[3,1];
RPT[2,2]←RT[2,3]*RP[3,2];
R[3,1]←RPT[3,1]←RT[3,3]*RP[3,1];
R[3,2]←RPT[3,2]←RT[3,3]*RP[3,2];
RS[3,3]←1;
RS[1,1]←RS[2,2]←COS(SWING[CLENS]);
RS[2,1]←-(RS[1,2]←SIN(SWING[CLENS]));
R[1,1]←RS[1,1]*RPT[1,1]+RS[1,2]*RPT[2,1];
R[1,2]←RS[1,1]*RPT[1,2]+RS[1,2]*RPT[2,2];
R[1,3]←RS[1,2]*RPT[2,3];
R[2,1]←RS[2,1]*RPT[1,1]+RS[2,2]*RPT[2,1];
R[2,2]←RS[2,1]*RPT[1,2]+RS[2,2]*RPT[2,2];
R[2,3]←RS[2,2]*RPT[2,3];
C[1]←P0[CLENS,1]+R[1,1]*DP[CLENS,1]+R[2,1]*DP[CLENS,2]+
R[3,1]*DP[CLENS,3];
C[2]←P0[CLENS,2]+R[1,2]*DP[CLENS,1]+R[2,2]*DP[CLENS,2]+
R[3,2]*DP[CLENS,3];
C[3]←P0[CLENS,3]+R[1,3]*DP[CLENS,1]+R[2,3]*DP[CLENS,2]+
R[3,3]*DP[CLENS,3];
⊃ CONTINUE COMPUTING TRANSFORM;
⊃ -FMX/333 and PP[2]-PP[1]/333 are corrections for
the non-orthogonality of the TV scan axes;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
COL[I,1]←R[I,1];
COL[I,2]←R[I,2];
ACC←0;
FOR J←1 STEP 1 UNTIL 3 DO ACC←ACC-R[I,J]*C[J];
COL[I,3]←ACC;
END;
FOR J←1 STEP 1 UNTIL 3 DO
BEGIN
COL[2,J]←-FMX/333*COL[1,J]+FMY*COL[2,J]
+(PP[CLENS,2]-PP[CLENS,1]/333)*COL[3,J];
COL[1,J]←FMX*COL[1,J]+PP[CLENS,1]*COL[3,J];
END;
INVRT(COL,ICOL);
ARRTRAN(CENTER,C);
END "PANTIL_CAM";
⊃ UPDATE THE TRANSFORM FOR SELECTED CAMERA;
SIMPLE MESSAGE PROCEDURE CAM_UPDATE(INTEGER CAMNO);
BEGIN
REAL SFOC,STIL,SPAN,IND,FMAX,FMIN,TMAX,TMIN,PMAX,PMIN,SZOM,ZMAX,
ZMIN,DIFFOC,DIFTIL,DIFPAN,DIFZOM,SIND;
EXTERNAL REAL L1,L2,L3,P1,P2,P3;
EXTERNAL INTEGER STATUS,TSERVO;
EXTERNAL REAL L4,L5,L6,L7,L8,P4,P5,P6,P7,P8;
EXTERNAL INTEGER STATE;
INTEGER UPDFLG;
FORTRAN REAL PROCEDURE SQRT(REAL X);
EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
EXTERNAL PROCEDURE SPWOFF;
EXTERNAL PROCEDURE scalpot;
LABEL ETA;
IF CAMNO=1 THEN
BEGIN
ETA: SFOC←STIL←SPAN←0;
FMAX←TMAX←PMAX←-10000;
FMIN←TMIN←PMIN←10000;
STATUS←1;
SPWON(1,TSERVO);
FOR IND←0 STEP 1 UNTIL 39 DO
BEGIN
STATUS←4;
WHILE ¬(STATUS LAND 1) DO;
IF (STATUS≥'100)∧(STATUS<'100000) THEN DONE;
SFOC←SFOC+P1;
STIL←STIL+P2;
SPAN←SPAN+P3;
IF P1>FMAX THEN FMAX←P1;
IF P1<FMIN THEN FMIN←P1;
IF P2>TMAX THEN TMAX←P2;
IF P2<TMIN THEN TMIN←P2;
IF P3>PMAX THEN PMAX←P3;
IF P3<PMIN THEN PMIN←P3;
END;
SPWOFF;
END;
⊃ UPDATE CONTINUES;
IF CAMNO=2 THEN
BEGIN
SFOC←SPAN←STIL←SZOM←0;
FMAX←TMAX←PMAX←ZMAX←-10000;
FMIN←TMIN←PMIN←ZMIN←10000;
FOR IND←0 STEP 1 UNTIL 39 DO
BEGIN
scalpot;
SPAN←SPAN+P4;
SFOC←SFOC+P6;
STIL←STIL+P5;
SZOM←SZOM+P7;
IF P4>PMAX THEN PMAX←P4;
IF P4<PMIN THEN PMIN←P4;
IF P5>TMAX THEN TMAX←P5;
IF P5<TMIN THEN TMIN←P5;
IF P6>FMAX THEN FMAX←P6;
IF P6<FMIN THEN FMIN←P6;
IF P7>ZMAX THEN ZMAX←P7;
IF P7<ZMIN THEN ZMIN←P7;
END;
END;
IF IND>0 THEN
BEGIN
ref← if camno=1 then cref else GROREF[5];
L1←FOCPOT←SFOC*REF/IND;
L2←TILPOT←STIL*REF/IND;
L3←PANPOT←SPAN*REF/IND;
IF CAMNO=2 THEN L7←ZOOPOT←SZOM*REF/IND;
IF DEB_CAM THEN
BEGIN
OUTSTR("CAM_UPDATE: FP="&CVG(FOCPOT)&" TP="&
CVG(TILPOT)&" PP="&CVG(PANPOT));
IF CAMNO=2 THEN OUTSTR(" ZP="&CVG(ZOOPOT)CRLF)
ELSE OUTSTR(" "CRLF);
END;
IF IND<30 THEN
BEGIN
OUTSTR("CAM_UPDATE: NOT ENOUGH READINGS "&
CVS(IND)&" "CRLF);
UPDFLG←8;
END ELSE BEGIN
DIFFOC←(FMAX-FMIN)*REF;
DIFTIL←(TMAX-TMIN)*REF;
DIFPAN←(PMAX-PMIN)*REF;
IF CAMNO=2 THEN DIFZOM←(ZMAX-ZMIN)*REF;
⊃ UPDATE CONTINUES;
IF DEB_CAM THEN
BEGIN
OUTSTR("CAM_UPDATE: "&"DF="&CVS(DIFFOC)&
" DT="&CVS(DIFTIL)&" DP="&
CVS(DIFPAN));
IF CAMNO=2 THEN OUTSTR(" DZ="&
CVS(DIFZOM)CRLF)
ELSE OUTSTR(" "CRLF);
END;
SIND←4*SQRT(IND);
IF (DIFPAN/SIND>.75)∨(DIFTIL/SIND>.75)∨
(DIFFOC/SIND>1)∨(CAMNO=2∧DIFZOM/SIND>1)
THEN BEGIN
OUTSTR("CAM_UPDATE: POTS TOO NOISY "&
CVF(DIFFOC)&" "&
CVF(DIFTIL)&" "&CVF(DIFPAN));
IF CAMNO=2 THEN OUTSTR(" "&
CVF(DIFZOM)CRLF)
ELSE OUTSTR(" "CRLF);
UPDFLG←9;
END;
END;
END ELSE BEGIN
OUTSTR("CAM_UPDATE: AD NOT AVAILABLE"CRLF);
UPDFLG←10;
END;
IF UPDFLG≠0 THEN
BEGIN
OUTSTR("...TYPE Y TO TRY AGAIN:"CRLF);
IF YES THEN BEGIN UPDFLG←0; GOTO ETA; END ELSE
IF (CAMFLG←UPDFLG)=10 THEN
BEGIN
OUTSTR("CAM_UPDATE-FAILED: CAMERA_MODEL NOT"&
" UPDATED"CRLF);
RETURN;
END;
END;
IF CAMNO=2 THEN CAMLENS←5;
PANTIL_CAM(CAMLENS,PANPOT,TILPOT,FOCPOT,ZOOPOT,MCOL,MICOL,LCEN);
IF CAMNO=2 THEN
BEGIN
FOC[5]←C1+ZOOPOT*C2;
FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
FOCLENG[5]←FPOTD[5]/KY;
L4←P4;
L5←P5;
L6←P6;
L6←P7;
L8←P8;
END;
⊃ UPDATE CONTINUES;
⊃ Now to update the global model;
ARRBLT (CAMERA_MODEL[1,1],MCOL[1,1],9);
ARRBLT (CAMERA_MODEL[6,1],MICOL[1,1],9);
ARRBLT (CAMERA_MODEL[4,1],LCEN[1],3);
CAMERA_MODEL[5,1] ← PP[CAMLENS,1];
CAMERA_MODEL[5,2] ← PP[CAMLENS,2];
CAMERA_MODEL[5,3] ← 1.0;
CAMERA_MODEL[9,1]←CAMPAN←PPOTD[CAMLENS]*PANPOT+PPOT0[CAMLENS];
CAMERA_MODEL[9,2]←CAMTIL←TPOTD[CAMLENS]*TILPOT+TPOT0[CAMLENS];
CAMERA_MODEL[9,3]←CAMRANG← LNS(FOCLEN0[CAMLENS]+
FOCLENG[CAMLENS]*FOCPOT,CAMLENS);
CAMERA_MODEL[10,2]←CAMLENS;
CAMERA_MODEL[10,1]←CAMNO;
CURCAM[camno] ← GLOBAL NEW (CAMERA_MODEL);
IF DEB_CAM THEN
BEGIN
OUTSTR("CAM_UPDATE: CAMPAN="&CVG(CAMPAN)&" CAMTIL="&
CVG(CAMTIL)&" CAMRANG="&CVG(CAMRANG));
IF CAMNO=2 THEN OUTSTR(CVG(FOC[5])CRLF)
ELSE OUTSTR(" "CRLF);
END;
END "CAM_UPDATE";
⊃ LENS CHANGES;
SIMPLE MESSAGE PROCEDURE CHNG_LENS(INTEGER RLENS);
BEGIN
EXTERNAL PROCEDURE CALLEN;
EXTERNAL INTEGER STATUS,LENS;
IF (RLENS<1)∨(RLENS>4) THEN
BEGIN
OUTSTR("CHNG_LENS-FAILED: LENS NO. OUT OF BOUNDS ("&
CVS(RLENS)&")"CRLF);
CAMFLG←2;
RETURN;
END;
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
LENS←RLENS-1;
STATUS←'20;
SERVO;
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
OUTSTR("CHNGE_LENS: I GOOFED, NOW YOU HAVE"&
" LENS NO. "&CVS(LENS+1)CRLF);
CAMFLG←6;
END;
END;
CAMLENS←LENS+1;
CAM_UPDATE(1);
END "CHNG_LENS";
⊃ MOVE PAN/TILT HEAD;
SIMPLE MESSAGE PROCEDURE MOVE_CAM(INTEGER CAMNO;REAL PAN,TILT);
BEGIN
REAL LL2,LL3;
EXTERNAL INTEGER STATUS,STATE;
EXTERNAL REAL L2,L3,L4,L5,L6,L7,L8;
EXTERNAL REAL P4,P5,P6,P7,P8;
ref← if camno=1 then cref else GROREF[5];
IF CAMNO=2 THEN CAMLENS←5;
LL3←(PAN-PPOT0[CAMLENS])/PPOTD[CAMLENS];
LL2←(TILT-TPOT0[CAMLENS])/TPOTD[CAMLENS];
IF CAMNO=1 THEN
BEGIN
L3←LL3/REF;
L2←LL2/REF;
END ELSE BEGIN
L4←LL3/REF;
L5←LL2/REF;
L6←P6;
L7←P7;
L8←P8;
END;
IF (LL2≥TLIMIT1[CAMNUM])∨(LL2≤TLIMIT2[CAMNUM]) THEN
BEGIN
OUTSTR("MOVE_CAM-FAILED: TILT OUT OF BOUNDS (LL2="&
CVS(LL2)&")"CRLF);
CAMFLG←4
END;
IF (LL3≥PLIMIT1[CAMNUM])∨(LL3≤PLIMIT2[CAMNUM]) THEN
BEGIN
OUTSTR("MOVE_CAM-FAILED: PAN OUT OF BOUNDS (LL3="&
CVS(LL3)&")"CRLF);
CAMFLG←3;
END;
IF CAMFLG≠0 THEN RETURN;
STATUS←'10;
SERVO;
CAM_UPDATE(camno);
END "MOVE_CAM";
⊃ CHANGE LENS FOCUS AND ZOOM;
SIMPLE MESSAGE PROCEDURE CHNG_FOCUS(INTEGER CAMNO;REAL RANG);
BEGIN
REAL LL1;
EXTERNAL INTEGER STATUS,STATE;
EXTERNAL REAL L1,L6;
EXTERNAL REAL L4,L5,L7,L8,P4,P5,P7,P8;
ref← if camno=1 then cref else GROREF[5];
IF CAMNO=2 THEN
BEGIN
CAMLENS←5;
FOCLENG[5]←FPOTD[5]/KY;
FOCLEN0[5]←FPOT0[5]/KY+ZPOTD/((ZOOPOT-ZPOT0)*KY);
L4←P4;
L5←P5;
L7←P7;
L8←P8;
END;
LL1←(LNS(RANG,CAMLENS)-FOCLEN0[CAMLENS])/FOCLENG[CAMLENS];
IF (LL1≥FLIMIT1[CAMNUM])∨(LL1≤FLIMIT2[CAMNUM]) THEN
BEGIN
OUTSTR("CHNGE_FOC-FAILED: FOCUS OUT OF BOUNDS (LL1="&
CVS(LL1)&")"CRLF);
CAMFLG←5;
RETURN;
END;
IF CAMNO=1 THEN L1←LL1/REF ELSE L6←LL1/REF;
STATUS←'10;
SERVO;
CAM_UPDATE(camno);
END "CHNG_FOCUS";
SIMPLE MESSAGE PROCEDURE CHNG_ZOOM(REAL FOC_LGTH);
BEGIN "CHNG_ZOOM"
EXTERNAL REAL L7;
EXTERNAL REAL L4,L5,L6,L8,P4,P5,P6,P8;
L4←P4;
L5←P5;
L6←P6;
L8←P8;
L7←(FOC_LGTH-C1)/(C2*REF);
SERVO;
CAM_UPDATE(2);
END "CHNG_ZOOM";
⊃ CENTER CAMERA;
SIMPLE MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC);
BEGIN
REAL XCC,YCC,ZCC,D,RANG,PAN,TILT,LL1,LL2,LL3;
EXTERNAL PROCEDURE CALLEN;
EXTERNAL INTEGER LENS,STATUS;
EXTERNAL REAL L1,L2,L3,L4,L5,L6,L7,L8;
FORTRAN REAL PROCEDURE SIN(REAL X);
FORTRAN REAL PROCEDURE COS(REAL X);
ref← if camno=1 then cref else sref;
IF CAMNO=1 THEN IF (RLENS<1)∨(RLENS>4) THEN
BEGIN
OUTSTR("CAM_CENTER-FAILED: LENS NO. OUT OF BOUNDS ("&
CVS(RLENS)&")"CRLF);
CAMFLG←2;
RETURN;
END;
IF CAMNO=2 THEN IF RLENS≠5 THEN
BEGIN
OUTSTR("WRONG LENS NUMBER FOR SIERRA"&CVS(RLENS)CRLF);
RETURN;
END;
XCC←P0[RLENS,1]-XTC;
YCC←P0[RLENS,2]-YTC;
ZCC←P0[RLENS,3]-ZTC;
PAN←COSQR(YCC,-XCC,DP[RLENS,1]);
IF (CAMNO=2) ∧ (P0[RLENS,2]+DP[RLENS,1]-YTC<0) THEN PAN←-PAN;
D←XCC*COS(PAN)+YCC*SIN(PAN);
TILT←COSQR(ZCC,-D,-DP[RLENS,2]);
RANG←D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];
LL3←(PAN-PPOT0[RLENS])/PPOTD[RLENS];
LL2←(TILT-TPOT0[RLENS])/TPOTD[RLENS];
LL1←(LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
IF (LL2≥TLIMIT1[CAMNO])∨(LL2≤TLIMIT2[CAMNO]) THEN
BEGIN
OUTSTR("CAM_CENTER¬FAILED: TILT OUT OF BOUNDS (LL2="&
CVE(LL2)&")"CRLF);
CAMFLG←4;
END;
IF (LL3≥PLIMIT1[CAMNO])∨(LL3≤PLIMIT2[CAMNO]) THEN
BEGIN
OUTSTR("CAM_CENTER-FAILED: PAN OUT OF BOUNDS (LL3="
&CVE(LL3)&")"CRLF);
CAMFLG←3;
END;
⊃ CAMERA CENTERING CONTINUES;
IF (LL1≥FLIMIT1[CAMNO])∨(LL1≤FLIMIT2[CAMNO]) THEN
BEGIN
OUTSTR("CAM_CENTER-FAILED: FOCUS OUT OF BOUNDS (LL1="&
CVE(LL1)&")"CRLF);
CAMFLG←5;
END;
IF CAMFLG≠0 THEN RETURN;
IF CAMNO=1 THEN GROREF[RLENS]←CREF;
L1←L6←LL1/GROREF[RLENS];
L2←L5←LL2/GROREF[RLENS];
L3←L4←LL3/GROREF[RLENS];
IF CAMNO=1 THEN
BEGIN
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
LENS←RLENS-1;
STATUS←'30;
END ELSE STATUS←'10;
SERVO;
CALLEN;
IF LENS≠RLENS-1 THEN
BEGIN
OUTSTR("CAM_CENTER: I GOOFED, NOW YOU HAVE"&
" LENS NO. "&CVS(LENS+1)CRLF);
CAMFLG←6;
END;
CAMLENS←LENS+1;
END ELSE SERVO;
CAM_UPDATE(camno);
END "CAM_CENTER";
⊃ TRANSFORM PREDICTION;
SIMPLE MESSAGE PROCEDURE CAM_PRED(INTEGER RLENS,CAMNO; REAL XTC,YTC,ZTC);
BEGIN
REAL XCC,YCC,ZCC,D,RANG,PAN,TILT,LL1,LL2,LL3;
FORTRAN REAL PROCEDURE SIN(REAL X);
FORTRAN REAL PROCEDURE COS(REAL X);
IF CAMNO=1 THEN IF (RLENS<1)∨(RLENS>4) THEN
BEGIN
OUTSTR("CAM_PRED-FAILED: LENS NO. OUT OF BOUNDS ("&
CVS(RLENS)&")"CRLF);
CAMFLG←2;
RETURN;
END ELSE IF RLENS≠5 THEN BEGIN
OUTSTR("CAM_PRED-FAILED: LENS NO. OUT OF BOUNDS"&
CVS(RLENS)CRLF);
RETURN;
END;
XCC←P0[RLENS,1]-XTC;
YCC←P0[RLENS,2]-YTC;
ZCC←P0[RLENS,3]-ZTC;
PAN←COSQR(YCC,-XCC,DP[RLENS,1]);
D←XCC*COS(PAN)+YCC*SIN(PAN);
TILT←COSQR(ZCC,-D,-DP[RLENS,2]);
RANG←D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];
LL3←(PAN-PPOT0[RLENS])/PPOTD[RLENS];
LL2←(TILT-TPOT0[RLENS])/TPOTD[RLENS];
LL1←(LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
IF (LL2≥TLIMIT1[RLENS])∨(LL2≤TLIMIT2[RLENS]) THEN
BEGIN
OUTSTR("CAM_PRED¬FAILED: TILT OUT OF BOUNDS (LL2="&
CVS(LL2)&")"CRLF);
CAMFLG←4;
END;
IF (LL3≥PLIMIT1[RLENS])∨(LL3≤PLIMIT2[RLENS]) THEN
BEGIN
OUTSTR("CAM_PRED-FAILED: PAN OUT OF BOUNDS (LL3="&
CVS(LL3)&")"CRLF);
CAMFLG←3;
END;
IF (LL1≥FLIMIT1[RLENS])∨(LL1≤FLIMIT2[RLENS]) THEN
BEGIN
OUTSTR("CAM_PRED-FAILED: FOCUS OUT OF BOUNDS (LL1="&
CVS(LL1)&")"CRLF);
CAMFLG←5;
END;
IF CAMFLG≠0 THEN RETURN;
⊃ CONTINUE PREDICTING, AND INITIALIZATION ROUTINE;
PANTIL_CAM (RLENS,LL3,LL2,LL1,ZOOPOT,MCOL,MICOL,LCEN);
⊃ temporary hack;
ARRBLT (CAMERA_PREDI[1,1],MCOL[1,1],9);
ARRBLT (CAMERA_PREDI[6,1],MICOL[1,1],9);
ARRBLT (CAMERA_PREDI[4,1],LCEN[1],3);
CAMERA_PREDI[5,1] ← PP[CAMLENS,1];
CAMERA_PREDI[5,2] ← PP[CAMLENS,2];
CAMERA_PREDI[5,3] ← 1.0;
CAMERA_PREDI[9,1]←PAN;
CAMERA_PREDI[9,2]←TILT;
CAMERA_PREDI[9,3]←RANG;
camera_predi[10,1]←camno;
CAMERA_PREDI[10,2]←RLENS;
END "CAM_PRED";
SIMPLE MESSAGE PROCEDURE CAM_INIT(INTEGER CAMNO);
BEGIN
CAMFLG←0;
IF CAMNO=1 THEN
FOR BLOCK←1 STEP 1 UNTIL 4 DO DATXFR;
IF CAMNO=2 THEN
BEGIN
BLOCK←1;
DATXFR;
END;
END"CAM_INIT";
⊃ PROGRAM TEST ROUTINE;
SIMPLE PROCEDURE TESTIT;
BEGIN
STRING COORD;
REAL X,Y,Z,NPAN,NTILT,NRANG;
INTEGER NLENS,REQUEST;
STRING ST;
LABEL TIT,TES;
TIT: OUTSTR("CAMERA NO.?"CRLF);
CAMNUM ← IF ST←INCHWL="1" THEN 1 ELSE 2;
OUTSTR("1-CHANGE THE LENS"CRLF&
"2-PAN AND TILT THE CAMERA"CRLF&
"3-FOCUS ON A GIVEN RANGE"CRLF&
"4-CENTER THE CAMERA ON A GIVEN POINT"CRLF&
"5-COMPLEMENT DEB_CAM"CRLF&
"6-CHANGE THE ZOOM"CRLF);
TES: OUTSTR("...TYPE THE NO. OF THE PROCEDUE YOU WANT TO TEST NOW="CRLF);
REQUEST←CVD(INCHWL);
IF (REQUEST<1)∨(REQUEST>6) THEN
BEGIN
OUTSTR("TESTIT-FAILED: ILLEGAL PROCEDURE NO ("&
CVS(REQUEST)&")"CRLF);
GOTO TIT;
END;
CASE REQUEST OF
BEGIN
; ⊃ 0;
BEGIN ⊃ 1;
OUTSTR("...TYPE NO. OF LENS="CRLF);
NLENS←CVD(INCHWL);
CHNG_LENS(NLENS);
END;
BEGIN ⊃ 2;
OUTSTR("...TYPE PAN AND TILT IN RADS,"&
" EACH FOLLOWED BY C.R="CRLF);
COORD←INCHWL;
NPAN←REALSCAN(COORD,MESS);
COORD←INCHWL;
NTILT←REALSCAN(COORD,MESS);
MOVE_CAM(CAMNUM,NPAN,NTILT);
END;
⊃ CONTINUE TESTING;
BEGIN ⊃ 3;
OUTSTR("...TYPE RANGE TO FOCUS ON="CRLF);
COORD←INCHWL;
NRANG←REALSCAN(COORD,MESS);
CHNG_FOCUS(CAMNUM,NRANG);
END;
BEGIN ⊃ 4;
OUTSTR("TYPE IN LENS N0."&
"AND X,Y,Z OF CENTER IN TABLE COORDINATES"CRLF&
"EACH FOLLOWED BY A C.R.="CRLF);
COORD←INCHWL;
NLENS←CVD(COORD);
COORD←INCHWL;
X←REALSCAN(COORD,MESS);
COORD←INCHWL;
Y←REALSCAN(COORD,MESS);
COORD←INCHWL;
Z←REALSCAN(COORD,MESS);
CAM_CENTER(NLENS,CAMNUM,X,Y,Z);
END;
BEGIN ⊃ 5; DEB_CAM←¬DEB_CAM; GOTO TES;END;
BEGIN ⊃ 6;
OUTSTR("DESIRED FOCAL LENGTH"CRLF);
COORD←INCHWL;
FOC_LGTH←REALSCAN(COORD,MESS);
CHNG_ZOOM(FOC_LGTH);
END;
END;
OUTSTR("...TYPE Y TO TEST AGAIN:"CRLF);
IF INCHWL="Y" THEN BEGIN CAMFLG←0; GOTO TIT; END;
END "TESTIT";
⊃ THIS IS THE MAIN PROGRAM;
CAMFLG←0;
TYP_CAM←TRUE;
DEB_CAM←FALSE;
YES_CAM←TRUE;
FOR CAMNUM←1 STEP 1 UNTIL 2 DO
BEGIN
CAM_INIT(CAMNUM);
HAT(CAMNUM);
END;
IF RUN=0 THEN
BEGIN
OUTSTR("...TYPE Y FOR TEST MODE:"CRLF);
IF YES THEN TESTIT;
END ;
PUT_DATA(0,0,"CAM"); ⊃ DECLARE YOUR NAME ;
OUTSTR("CAM-ACTIVATED"CRLF);
WHILE TRUE DO
BEGIN
MESS ← GET_ENTRY ('120,NULL,"CAM",NULL);
CAMFLG←0;
MESS←QUEUE('600,MESS); ⊃ ACTIVATE AND ACKNOWLEDGE ;
END;
END "GILCAM";